#include "action.h"
#include "servo.h"
#include "dji2_0.h"
/**
* 前爪 闭=20 开=120
* 后爪 闭=160 开=10
* 门 闭=28 开=36
*/

#define ANG_DOOR_OPEN           36 
#define ANG_DOOR_CLOSE          28

#define ANG_ARM_FRONT_OPEN      120
#define ANG_ARM_BACK_OPEN       10
#define ANG_ARM_FRONT_CLOSE     10
#define ANG_ARM_BACK_CLOSE      170

#define POS_PLATFORM_OUT        -200000
#define POS_PLATFORM_IN         0

#define ANG_SIDECAM_LEFT        0   //TODO: 需要测量
#define ANG_SIDECAM_RIGHT       180 //TODO: 需要测量

#define ANG_FLAG_1              1
#define ANG_FLAG_2              90
#define ANG_FLAG_3              180

// TIM4-CHANNEL4 (PWM1)
void action_door(uint8_t mode)
{
    if (mode == 1) {
        servo_set_angle(&htim4, TIM_CHANNEL_4, ANG_DOOR_OPEN);
    }
    else if (mode == 0) {
        servo_set_angle(&htim4, TIM_CHANNEL_4, ANG_DOOR_CLOSE);
    }
}

// Front: TIM4-CHANNEL3 (PWM2) Back: TIM4-CHANNEL2(PWM3);
void action_arm(uint8_t mode)
{
    if (mode == 1) {
        servo_set_angle(&htim4, TIM_CHANNEL_3, ANG_ARM_FRONT_OPEN);
        servo_set_angle(&htim4, TIM_CHANNEL_2, ANG_ARM_BACK_OPEN);
    }
    else if (mode == 0) {
        servo_set_angle(&htim4, TIM_CHANNEL_3, ANG_ARM_FRONT_CLOSE);
        servo_set_angle(&htim4, TIM_CHANNEL_2, ANG_ARM_BACK_CLOSE);
    }
}

// 要求上电时平台处于收回状态
void action_platform(uint8_t mode)
{
    if (mode == 1) {
        dji_PosCtrl(&hcan1, 1, 1, POS_PLATFORM_OUT);
    }
    else if (mode == 0) {
        dji_PosCtrl(&hcan1, 1, 1, POS_PLATFORM_IN);
    }
}

// TIM4-CHANNEL1 (PWM4)
void action_sidecam(uint8_t dir)
{
    if (dir == 1) {
        servo_set_angle(&htim4, TIM_CHANNEL_1, ANG_SIDECAM_LEFT);
    }
    else if (dir == 0) {
        servo_set_angle(&htim4, TIM_CHANNEL_1, ANG_SIDECAM_RIGHT);
    }
}

// TIM8-CHANNEL4 (PWM5)
void action_flag(uint8_t which)
{
    switch (which) {
    case 1:
        servo_set_angle(&htim8, TIM_CHANNEL_4, ANG_FLAG_1);
        break;
    case 2:
        servo_set_angle(&htim8, TIM_CHANNEL_4, ANG_FLAG_2);
        break;
    case 3:
        servo_set_angle(&htim8, TIM_CHANNEL_4, ANG_FLAG_3);
        break;
    default:
        break;
    }
}